#pragma once
#include "debug.hh"
#include "vec2.hh"
#include <iostream>

namespace tue
{
    namespace geometry_impl
    {
        /*
        for (extendable) segments:
            points[0]  points[1]
                 |        |
            -----*------->*----- extb, extf
                 *------->*----- extf
            -----*------->*      extb
                 *------->*

            point  = points[0]
            dir    = normalize(points[1] - points[0])
            radius = |points[1] - points[0]|

        for circle:
            point -> radius
            dir = (+inf, +inf)
        */
        struct geometry
        {
            vec2 point;
            vec2 dir;
            double radius;
            bool extend_backward;
            bool extend_forward;

            // segment
            geometry(std::istream & i, bool extb, bool extf)
                : extend_backward(extb)
                , extend_forward(extf)
            {
                i >> point >> dir;
                dir -= point;
                radius = len(dir);
                dir /= radius;
            }

            geometry(std::istream & i) : dir(INFINITY, INFINITY)
            {
                i >> point >> radius;
            }

            vec2 at(double k) const { return point + k*dir; }
            bool is_circle() const { return isinf(dir.real()); }

            // assume p is on the line point+k*dir
            // check if p is also on the extendable segment
            bool contains(vec2 const& p) const { return contains(dot(p-point, dir)); }

            // assume point+k*dir is a point on the line,
            // check if it is also on the extendable segment
            bool contains(double k) const
            {
                if (!extend_backward && k < -tolerance       ) return false;
                if (!extend_forward  && k > +tolerance+radius) return false;
                return true;
            }
        };

        template <class OUTPUT_IT>
        inline void intersect_ss(OUTPUT_IT out, geometry const& a, geometry const& b)
        {
            double k = intersect(a.point, a.dir, b.point, b.dir);
            if (isinf(k)) return;
            if (!a.contains(k)) return;
            vec2 p = a.at(k);
            if (!b.contains(p)) return;
            *out++ = p;
        }

        template <class OUTPUT_IT>
        inline void intersect_sc(OUTPUT_IT out, geometry const& s, geometry const& c)
        {
            double k0, k1;
            intersect(k0, k1, s.point, s.dir, c.point, c.radius);
            if (!isinf(k0) && s.contains(k0)) *out++ = s.at(k0);
            if (!isinf(k1) && s.contains(k1)) *out++ = s.at(k1);
        }

        template <class OUTPUT_IT>
        inline void intersect_cc(OUTPUT_IT out, geometry const& c0, geometry const& c1)
        {
            if (c0.radius < c1.radius) return intersect_cc(out, c1, c0);

            vec2 p = c1.point - c0.point;
            double d = len(p);
            double c = intersect(d, c0.radius, c1.radius);

            if (isinf(c)) return;

            p *= c0.radius / d;
            if (c == 1) {
                *out++ = c0.point + p;
                return;
            }

            vec2 rr = rotator(c);
            *out++ = c0.point + p * rr;
            *out++ = c0.point + p * std::conj(rr);
        }

        // dispatcher
        template <class OUTPUT_IT>
        inline void intersect(OUTPUT_IT out, geometry const& a, geometry const& b)
        {
            bool ac = a.is_circle();
            bool bc = b.is_circle();
            if (ac && bc) return intersect_cc(out, a, b);
            if (!ac && !bc) return intersect_ss(out, a, b);
            if (ac) return intersect_sc(out, b, a);
            /* bc */ return intersect_sc(out, a, b);
        }
    }
    using geometry_impl::geometry;
}

